#include "cv.h"
#include "highgui.h"
#include <iostream>
#include <cstdlib>
#include <string>
#include "Filter.h"
#include "Kalman.h"
#include "Platform.h"
#include "AlvarException.h"
using namespace alvar;
using namespace std;

const int res = 320;

void filter_none(double x, double y, double* fx, double* fy)
{
  *fx = x;
  *fy = y;
}

void filter_average(double x, double y, double* fx, double* fy)
{
  static FilterAverage ax(30);
  static FilterAverage ay(30);
  *fx = ax.next(x);
  *fy = ay.next(y);
}

void filter_median(double x, double y, double* fx, double* fy)
{
  static FilterMedian ax(30);
  static FilterMedian ay(30);
  *fx = ax.next(x);
  *fy = ay.next(y);
}

void filter_running_average(double x, double y, double* fx, double* fy)
{
  static FilterRunningAverage ax(0.03);
  static FilterRunningAverage ay(0.03);
  *fx = ax.next(x);
  *fy = ay.next(y);
}

void filter_des(double x, double y, double* fx, double* fy)
{
  static FilterDoubleExponentialSmoothing ax(0.03, 0.01);
  static FilterDoubleExponentialSmoothing ay(0.03, 0.01);
  *fx = ax.next(x);
  *fy = ay.next(y);
}

void filter_kalman(double x, double y, double* fx, double* fy)
{
  static bool init = true;
  static KalmanSensor sensor(4, 2);
  static Kalman kalman(4);  // x, y, dx, dy
  if (init)
  {
    init = false;
    // H
    cvZero(sensor.H);
    cvmSet(sensor.H, 0, 0, 1);
    cvmSet(sensor.H, 1, 1, 1);
    // R
    cvSetIdentity(sensor.R, cvScalar(10));
    // F
    cvSetIdentity(kalman.F);
    cvmSet(kalman.F, 0, 2, 1);
    cvmSet(kalman.F, 1, 3, 1);
    // Q
    cvmSet(kalman.Q, 0, 0, 0.0001);
    cvmSet(kalman.Q, 1, 1, 0.0001);
    cvmSet(kalman.Q, 2, 2, 0.000001);
    cvmSet(kalman.Q, 3, 3, 0.000001);
    // P
    cvSetIdentity(kalman.P, cvScalar(100));
  }
  cvmSet(sensor.z, 0, 0, x);
  cvmSet(sensor.z, 1, 0, y);
  kalman.predict_update(
      &sensor,
      (unsigned long)(cv::getTickCount() / cv::getTickFrequency() * 1000));
  *fx = cvmGet(kalman.x, 0, 0);
  *fy = cvmGet(kalman.x, 1, 0);
}

void filter_array_average(double x, double y, double* fx, double* fy)
{
  static bool init = true;
  static FilterArray<FilterAverage> fa(2);
  if (init)
  {
    init = false;
    for (int i = 0; i < 2; i++)
    {
      fa[i].setWindowSize(30);
    }
  }
  *fx = fa[0].next(x);
  *fy = fa[1].next(y);
}

class KalmanSensorOwn : public KalmanSensorEkf
{
  virtual void h(CvMat* x_pred, CvMat* _z_pred)
  {
    double x = cvmGet(x_pred, 0, 0);
    double y = cvmGet(x_pred, 1, 0);
    double dx = cvmGet(x_pred, 2, 0);
    double dy = cvmGet(x_pred, 3, 0);
    cvmSet(_z_pred, 0, 0, x);
    cvmSet(_z_pred, 1, 0, y);
  }

public:
  KalmanSensorOwn(int _n, int _m) : KalmanSensorEkf(_n, _m)
  {
  }
};

class KalmanOwn : public KalmanEkf
{
  virtual void f(CvMat* _x, CvMat* _x_pred, double dt)
  {
    double x = cvmGet(_x, 0, 0);
    double y = cvmGet(_x, 1, 0);
    double dx = cvmGet(_x, 2, 0);
    double dy = cvmGet(_x, 3, 0);
    cvmSet(_x_pred, 0, 0, x + dt * dx);
    cvmSet(_x_pred, 1, 0, y + dt * dy);
    cvmSet(_x_pred, 2, 0, dx);
    cvmSet(_x_pred, 3, 0, dy);
  }

public:
  KalmanOwn(int _n) : KalmanEkf(_n)
  {
  }
};

void filter_ekf(double x, double y, double* fx, double* fy)
{
  static bool init = true;
  static KalmanSensorOwn sensor(4, 2);
  static KalmanOwn kalman(4);  // x, y, dx, dy
  if (init)
  {
    init = false;
    // R
    cvSetIdentity(sensor.R, cvScalar(100));
    // Q
    cvmSet(kalman.Q, 0, 0, 0.001);
    cvmSet(kalman.Q, 1, 1, 0.001);
    cvmSet(kalman.Q, 2, 2, 0.01);
    cvmSet(kalman.Q, 3, 3, 0.01);
    // P
    cvSetIdentity(kalman.P, cvScalar(100));
  }
  cvmSet(sensor.z, 0, 0, x);
  cvmSet(sensor.z, 1, 0, y);
  kalman.predict_update(
      &sensor,
      (unsigned long)(cv::getTickCount() / cv::getTickFrequency() * 1000));
  *fx = cvmGet(kalman.x, 0, 0);
  *fy = cvmGet(kalman.x, 1, 0);
}

// Make list of filters
const int nof_filters = 8;
void (*(filters[nof_filters]))(double x, double y, double* fx, double* fy) = {
  filter_none, filter_average, filter_median, filter_running_average,
  filter_des,  filter_kalman,  filter_ekf,    filter_array_average,
};
char filter_names[nof_filters][64] = { "No filter - Press any key to change",
                                       "Average",
                                       "Median",
                                       "Running Average",
                                       "Double Exponential Smoothing",
                                       "Kalman",
                                       "Extended Kalman",
                                       "Array (average)" };

// Just generate some random data that can be used as sensor input
void get_measurement(double* x, double* y)
{
  static double xx = 0;
  static double yy = 0;
  static double dxx = 0.3;
  static double dyy = 0.7;
  xx += dxx;
  yy += dyy;
  if ((xx > res) || (xx < 0))
    dxx = -dxx;
  if ((yy > res) || (yy < 0))
    dyy = -dyy;
  double rx = (rand() * 20.0 / RAND_MAX) - 10.0;
  double ry = (rand() * 20.0 / RAND_MAX) - 10.0;

  // Add some outliers
  if (fabs(rx * ry) > 50)
  {
    rx *= 5;
    ry *= 5;
  }

  *x = xx + rx;
  *y = yy + ry;
}

int main(int argc, char* argv[])
{
  try
  {
    // Output usage message
    std::string filename(argv[0]);
    filename = filename.substr(filename.find_last_of('\\') + 1);
    std::cout << "SampleFilter" << std::endl;
    std::cout << "============" << std::endl;
    std::cout << std::endl;
    std::cout << "Description:" << std::endl;
    std::cout << "  This is an example of how to use the 'FilterAverage', "
                 "'FilterMedian',"
              << std::endl;
    std::cout << "  'FilterRunningAverage', "
                 "'FilterDoubleExponentialSmoothing', 'Kalman'"
              << std::endl;
    std::cout << "  'KalmanEkf' and 'FilterArray' filtering classes. First the "
                 "example"
              << std::endl;
    std::cout << "  shows unfiltered test data with outliers. The data is then "
                 "filtered"
              << std::endl;
    std::cout << "  using the various filters. Press any key to cycle through "
                 "the filters."
              << std::endl;
    std::cout << std::endl;
    std::cout << "Usage:" << std::endl;
    std::cout << "  " << filename << std::endl;
    std::cout << std::endl;
    std::cout << "Keyboard Shortcuts:" << std::endl;
    std::cout << "  any key: cycle through filters" << std::endl;
    std::cout << "  q: quit" << std::endl;
    std::cout << std::endl;

    // Processing loop
    IplImage* img = cvCreateImage(cvSize(res, res), IPL_DEPTH_8U, 3);
    cvNamedWindow("SampleFilter");
    CvFont font;
    cvInitFont(&font, CV_FONT_HERSHEY_PLAIN, 1.0, 1.0);
    for (int ii = 0; ii < nof_filters; ii++)
    {
      int key = 0;
      double x, y;
      double fx, fy;
      vector<CvPoint> tail;
      while (1)
      {
        get_measurement(&x, &y);
        filters[ii](x, y, &fx, &fy);
        cvZero(img);
        cvPutText(img, filter_names[ii], cvPoint(3, res - 10), &font,
                  CV_RGB(255, 255, 255));
        cvCircle(img, cvPoint(int(x), int(y)), 2, CV_RGB(0, 255, 255));
        cvCircle(img, cvPoint(int(x), int(y)), 3, CV_RGB(255, 255, 255));
        CvPoint fp;
        fp.x = int(fx);
        fp.y = int(fy);
        tail.push_back(fp);
        for (size_t iii = 0; iii < tail.size(); iii++)
        {
          cvCircle(img, tail[iii], 0, CV_RGB(255, 255, 0));
        }
        cvCircle(img, fp, 2, CV_RGB(255, 0, 255));
        cvShowImage("SampleFilter", img);
        key = cvWaitKey(10);
        if (key != -1)
        {
          break;
        }
      }
      if (key == 'q')
      {
        break;
      }
    }
    cvReleaseImage(&img);
    return 0;
  }
  catch (const std::exception& e)
  {
    std::cout << "Exception: " << e.what() << endl;
  }
  catch (...)
  {
    std::cout << "Exception: unknown" << std::endl;
  }
}
